﻿#include "MePtsHolder.h"

#include <math.h>
#include <algorithm>
#include <vector>
#include <memory.h>
#include <iostream>

#include <QString>
#include <QStringList>
#include "memath/memath.h"

#include <memath/MeTransformCoord.h>

/// 比较MePoint3D X方向
static bool cmpMePointX(const MePoint3D &a, const MePoint3D &b)
{
    return a.v[0] < b.v[0]; //由小到大排序
}
/// 比较MePoint3D Y方向
static bool cmpMePointYDsc(const MePoint3D &a, const MePoint3D &b)
{
    return a.v[1] > b.v[1]; //由小到大排序
}

MePtsHolder::MePtsHolder()
{
    pts.resize(4);
    disPredef.resize(6);
}

MePtsHolder::MePtsHolder(double _dPrefdef[])
{
    disPredef.resize(6);
    for (int i = 0; i< 6; i++) {
        disPredef[i] = _dPrefdef[i];
    }
}

void MePtsHolder::setPredefDistance(double _dPredef[])
{
    for (int i = 0; i< 6; i++) {
        disPredef[i] = _dPredef[i];
    }
    calPtsByDisPredef(pts.data());
}

int MePtsHolder::matchPoints(std::vector<MePoint3D> &inPts, std::vector<int> &outPtsIndex, std::vector<MePoint3D> &outPts, const double eps)
{
    outPts.resize(4);
    outPtsIndex.resize(4);
    return MePtsHolder::calPt4PFromPefDefDis(disPredef, inPts, outPtsIndex.data(), outPts.data(), eps);
}

int MePtsHolder::calPt4PFromPefDefDis(double prefDefDis[], double inPs[], int inPsSize, int outPt4pIndex[], double outPt4P[], const double eps)
{
    std::vector<PointIndexPair> vectorPair;
    for (int i = 0; i < inPsSize; i++) {
        vectorPair.clear();
        // 1. 计算 i 和 他后面的距离
        for (int j=i+1; j < inPsSize; j++) {
            // distance [i, j]
            double d = MeMath::distance(inPs+3*i, inPs+3*j);
            int index = getPredefDisIndex(prefDefDis, d);
            if (index != -1) {
                // 保存 d, i, j, index
                PointIndexPair pp(index, d, i, j);
                vectorPair.push_back(pp);
            }
        }

        //2. 有3个以上，可能有符合要求的点，否则抛弃i点继续啊
        if (vectorPair.size() >= 3) {
            // 找到需要的是个点
            int *pt4pIndex = outPt4pIndex;
            if (MePtsHolder::findCorrectPt4PIndex(prefDefDis, inPs, vectorPair, pt4pIndex)) {
                for (int k= 0; k<4; k++) {
                    outPt4P[3*k+0] = inPs[3*pt4pIndex[k] + 0];
                    outPt4P[3*k+1] = inPs[3*pt4pIndex[k] + 1];
                    outPt4P[3*k+2] = inPs[3*pt4pIndex[k] + 2];
                }
                return 4;
            }
        }
    }

    return 0;
}

int MePtsHolder::calPt4PFromPefDefDis(double prefDefDis[], std::vector<MePoint3D> &inPts, int outPt4pIndex[], MePoint3D outPts[], const double eps)
{
    std::vector<PointIndexPair> vectorPair;
    size_t inPsSize = inPts.size();
    for (size_t i = 0; i < inPsSize; i++) {
        vectorPair.clear();
        // 1. 计算 i 和 j 的距离 j = [i+1, size-1]
        for (size_t j=i+1; j < inPsSize; j++) {
            // distance [i, j]
            double d = MePoint3D::distance(inPts[i], inPts[j]);
            int index = getPredefDisIndex(prefDefDis, d);
            if (index != -1) {
                // 保存 d, i, j, index
                PointIndexPair pp(index, d, (int)i, (int)j);
                vectorPair.push_back(pp);
            }
        }

        //2. 有3个以上，可能有符合要求的点，否则抛弃i点继续啊
        if (vectorPair.size() >= 3) {
            // 找到需要的是个点
            int *pt4pIndex = outPt4pIndex;
            if (MePtsHolder::findCorrectPt4PIndex(prefDefDis, inPts, vectorPair, pt4pIndex)) {
                for (int k= 0; k<4; k++) {
                    outPts[k] = inPts[(size_t)pt4pIndex[k]];
                }
                return 4;
            }
        }
    }

    return 0;
}

int MePtsHolder::calPt4PFromPefDefDis(std::vector<double> prefDefDis, std::vector<MePoint3D> &inPts, int outPt4pIndex[], MePoint3D outPts[], const double eps)
{
    assert(prefDefDis.size() >= 6);
    return calPt4PFromPefDefDis(prefDefDis.data(), inPts, outPt4pIndex, outPts, eps);
}

void MePtsHolder::calHolderTransformWith3Pt(std::vector<MePoint3D> &baseHolderPts, std::vector<MePoint3D> &holderYPts, Eigen::Matrix3d &R_YtoB, Eigen::Vector3d &T_YtoB)
{
    // 计算 nPose 个 矩阵，使用 [p1, p2, p3]
    MePoint3D baseHolderPts_3p[3] = {
        baseHolderPts[0], baseHolderPts[1], baseHolderPts[2]
    };
    MePoint3D holderPts_3p[3] = {
        holderYPts[0], holderYPts[1], holderYPts[2]
    };
    MeTransformCoord::rigidTransform3D(baseHolderPts_3p, holderPts_3p, 3, R_YtoB, T_YtoB);
}

void MePtsHolder::calPt4PDistance(double inP[], double outDis[])
{
    calPt4PDistance(inP+0, inP+3, inP+6, inP+9, outDis);
}

void MePtsHolder::calPt4PDistance(double p1[], double p2[], double p3[], double p4[], double outDis[])
{
    double *p[4];
    p[0] = p1;
    p[1] = p2;
    p[2] = p3;
    p[3] = p4;

    int dIndex = 0;
    for (int i = 0;i < 4;i++)
    {
        for (int j = i+1; j < 4; j++) {
            // 计算两点之间的距离
            outDis[dIndex++] = MeMath::distance(p[i], p[j]);
        }
    }
}

void MePtsHolder::calPt4PDistance(MePoint3D points[4], double outDis[6])
{
    calPt4PDistance(points[0].v, points[1].v, points[2].v, points[3].v, outDis);
}

int MePtsHolder::getPredefDisIndex(double pefDefDis[], double dis)
{
    for (int i = 0; i < 6; ++i) {
        if (isInError(pefDefDis[i] - dis)) {
            return i;
        }
    }
    return -1;
}

bool MePtsHolder::isPt4PWithPredefDis(double prefDefDis[], double p1[], double p2[], double p3[], double p4[])
{
    double dis[6];
    calPt4PDistance(p1, p2, p3, p4, dis);
    for (int i=0; i< 6;i++) {
        if (!isInError(dis[i] - prefDefDis[i])) {
            return false;
        }
    }
    return true;
}

bool MePtsHolder::isPt4PWithPredefDis(double prefDefDis[], MePoint3D points[4])
{
    double dis[6];
    calPt4PDistance(points, dis);
    for (int i=0; i< 6;i++) {
        if (!isInError(dis[i] - prefDefDis[i])) {
            return false;
        }
    }
    return true;
}

/// 遍历所有，匹配到可能的点的索引
bool MePtsHolder::findCorrectPt4PIndex(double prefDefDis[], double inPs[], std::vector<PointIndexPair> vectorPair, int pt4pIndex[])
{
    // 输入距离为 {p1p2, p1p3, p1p4, p2p3, p2p4, p3p4}
    //           {0,    1,    2,    3,    4,    5}
    // p1 {0, 1, 2}  p1点对应的索引
    // p2 {0, 3, 4}
    // p3 {1, 3, 5}
    // p4 {2, 4, 5}

    // 按照index排序
    std::sort(vectorPair.begin(), vectorPair.end(), PointIndexPair::compPointIndexPair);
    int count = (int)vectorPair.size();

    for(unsigned int i = 0; (int)i < count-2; i++) {
        for (unsigned int j=i+1; (int)j<count-1; j++) {
            for (unsigned int k=j+1; (int)k< count; k++) {
                // 取出 i, j, k
                if ((vectorPair[i].index == 0) && (vectorPair[j].index == 1) && (vectorPair[k].index == 2)) {
                    // p1 {0, 1, 2}
                    pt4pIndex[0] = vectorPair[i].i;
                    pt4pIndex[1] = vectorPair[i].j;
                    pt4pIndex[2] = vectorPair[j].j;
                    pt4pIndex[3] = vectorPair[k].j;

                    if (isPt4PWithPredefDis(prefDefDis, inPs+3*pt4pIndex[0], inPs+3*pt4pIndex[1], inPs+3*pt4pIndex[2], inPs+3*pt4pIndex[3])) {
                        return true;
                    }
                } else if ( (vectorPair[i].index == 0) && (vectorPair[j].index == 3) && (vectorPair[k].index == 4) ) {
                    // p2 {0, 3, 4}
                    pt4pIndex[0] = vectorPair[i].j;
                    pt4pIndex[1] = vectorPair[i].i;
                    pt4pIndex[2] = vectorPair[j].j;
                    pt4pIndex[3] = vectorPair[k].j;
                    if (isPt4PWithPredefDis(prefDefDis, inPs+3*pt4pIndex[0], inPs+3*pt4pIndex[1], inPs+3*pt4pIndex[2], inPs+3*pt4pIndex[3])) {
                        return true;
                    }
                } else if ( (vectorPair[i].index == 1) && (vectorPair[j].index == 3) && (vectorPair[k].index == 5) ) {
                    // p3 {1, 3, 5}
                    pt4pIndex[0] = vectorPair[i].j;
                    pt4pIndex[1] = vectorPair[j].j;
                    pt4pIndex[2] = vectorPair[i].i;
                    pt4pIndex[3] = vectorPair[k].j;
                    if (isPt4PWithPredefDis(prefDefDis, inPs+3*pt4pIndex[0], inPs+3*pt4pIndex[1], inPs+3*pt4pIndex[2], inPs+3*pt4pIndex[3])) {
                        return true;
                    }
                } else if ( (vectorPair[i].index == 2) && (vectorPair[j].index == 4) && (vectorPair[k].index == 5) ) {
                    // p4 {2, 4, 5}
                    pt4pIndex[0] = vectorPair[i].j;
                    pt4pIndex[1] = vectorPair[j].j;
                    pt4pIndex[2] = vectorPair[k].j;
                    pt4pIndex[3] = vectorPair[i].i;
                    if (isPt4PWithPredefDis(prefDefDis, inPs+3*pt4pIndex[0], inPs+3*pt4pIndex[1], inPs+3*pt4pIndex[2], inPs+3*pt4pIndex[3])) {
                        return true;
                    }
                }
            }
        }
    }

    return false;
}

/// 遍历所有，匹配到可能的点的索引
bool MePtsHolder::findCorrectPt4PIndex(double prefDefDis[], std::vector<MePoint3D> &inPts, std::vector<PointIndexPair> vectorPair, int pt4pIndex[])
{
    // 输入距离为 {p1p2, p1p3, p1p4, p2p3, p2p4, p3p4}
    //           {0,    1,    2,    3,    4,    5}
    // p1 {0, 1, 2}  p1点对应的索引
    // p2 {0, 3, 4}
    // p3 {1, 3, 5}
    // p4 {2, 4, 5}

    // 按照index排序
    std::sort(vectorPair.begin(), vectorPair.end(), PointIndexPair::compPointIndexPair);
    int count = (int)vectorPair.size();

    for(unsigned int i = 0; (int)i < count-2; i++) {
        for (unsigned int j=i+1; (int)j<count-1; j++) {
            for (unsigned int k=j+1; (int)k< count; k++) {
                // 取出 i, j, k
                if ((vectorPair[i].index == 0) && (vectorPair[j].index == 1) && (vectorPair[k].index == 2)) {
                    // p1 {0, 1, 2}
                    pt4pIndex[0] = vectorPair[i].i;
                    pt4pIndex[1] = vectorPair[i].j;
                    pt4pIndex[2] = vectorPair[j].j;
                    pt4pIndex[3] = vectorPair[k].j;

                    if (isPt4PWithPredefDis(prefDefDis, inPts[pt4pIndex[0]].v, inPts[pt4pIndex[1]].v, inPts[pt4pIndex[2]].v, inPts[pt4pIndex[3]].v)) {
                        return true;
                    }
                } else if ( (vectorPair[i].index == 0) && (vectorPair[j].index == 3) && (vectorPair[k].index == 4) ) {
                    // p2 {0, 3, 4}
                    pt4pIndex[0] = vectorPair[i].j;
                    pt4pIndex[1] = vectorPair[i].i;
                    pt4pIndex[2] = vectorPair[j].j;
                    pt4pIndex[3] = vectorPair[k].j;
                    if (isPt4PWithPredefDis(prefDefDis, inPts[pt4pIndex[0]].v, inPts[pt4pIndex[1]].v, inPts[pt4pIndex[2]].v, inPts[pt4pIndex[3]].v)) {
                        return true;
                    }
                } else if ( (vectorPair[i].index == 1) && (vectorPair[j].index == 3) && (vectorPair[k].index == 5) ) {
                    // p3 {1, 3, 5}
                    pt4pIndex[0] = vectorPair[i].j;
                    pt4pIndex[1] = vectorPair[j].j;
                    pt4pIndex[2] = vectorPair[i].i;
                    pt4pIndex[3] = vectorPair[k].j;
                    if (isPt4PWithPredefDis(prefDefDis, inPts[pt4pIndex[0]].v, inPts[pt4pIndex[1]].v, inPts[pt4pIndex[2]].v, inPts[pt4pIndex[3]].v)) {
                        return true;
                    }
                } else if ( (vectorPair[i].index == 2) && (vectorPair[j].index == 4) && (vectorPair[k].index == 5) ) {
                    // p4 {2, 4, 5}
                    pt4pIndex[0] = vectorPair[i].j;
                    pt4pIndex[1] = vectorPair[j].j;
                    pt4pIndex[2] = vectorPair[k].j;
                    pt4pIndex[3] = vectorPair[i].i;
                    if (isPt4PWithPredefDis(prefDefDis, inPts[pt4pIndex[0]].v, inPts[pt4pIndex[1]].v, inPts[pt4pIndex[2]].v, inPts[pt4pIndex[3]].v)) {
                        return true;
                    }
                }
            }
        }
    }

    return false;
}

/// predis 预先知道的6个距离，输出4个点
/// 设定z=0， Y支架，右边高左边点， P2点为Y中间点， P1为Y下端点，P2P1为X正方向，
/// z轴从纸面出来，左边支点P3， 右边支点P4
MePtsHolderTypeY::MePtsHolderTypeY()
{
    holderType = MePtsHolder::HolderTypeY;
    // 预先定义的值
//    static double distancePredefDefault[6] = {81.4059, 77.959, 157.416, 92.814, 176.72, 86.998};
    static double distancePredefDefault[6] = { 86.0292,155.437,174.839,77.0428,91.7739,80.5978 };
    memcpy(disPredef.data(), distancePredefDefault, sizeof(double) * 6);
//    for (int i = 0; i< 6; i++) {
//        disPredef[i] = distancePredefDefault[i];
//    }
    calPtsByDisPredef(pts.data());
}

MePtsHolderTypeY::MePtsHolderTypeY(double _dPrefdef[])
{
    holderType = MePtsHolder::HolderTypeY;
    memcpy(disPredef.data(), _dPrefdef, sizeof(double) * 6);
    calPtsByDisPredef(pts.data());
}

void MePtsHolderTypeY::calPtsByDisPredef(MePoint3D points[4])
{
    // 输入距离为 {p1p2, p1p3, p1p4, p2p3, p2p4, p3p4}
    //           {0,    1,    2,    3,    4,    5}
    calPtsByDisPredefOp3(points);
}

void MePtsHolderTypeY::calPtsByDisPredef(MePoint3D points[], double p3ToPlane)
{
    // 输入距离为 {p1p2, p1p3, p1p4, p2p3, p2p4, p3p4}
    //           {0,    1,    2,    3,    4,    5}
    // p2为原点
    points[1].v[0] = 0.0;
    points[1].v[1] = 0.0;
    points[1].v[2] = 0.0;

    // p1在X轴上
    points[0].v[0] = disPredef[0];  // p3p4 距离
    points[0].v[1] = 0.0;
    points[0].v[2] = 0.0;

    // 计算p3 夹角 p3p2p1为theta1
    double a = disPredef[3];
    double b = disPredef[0];
    double c = disPredef[1];
    double theta1 = acos((a*a+b*b-c*c)/(2*a*b));  // 余弦定理
    points[2].v[0] = a * cos(theta1);
    points[2].v[1] = -a * sin(theta1);
    points[2].v[2] = p3ToPlane;

    // p1
    a = disPredef[4];
    c = disPredef[2];
    double theta2 = acos((a*a+b*b-c*c)/(2*a*b));  // 余弦定理
    points[3].v[0] = a * cos(theta2);
    points[3].v[1] = a * sin(theta2);
    points[3].v[2] = 0.0;
}

void MePtsHolderTypeY::calPtsByDisPredefOp3(MePoint3D points[])
{
    // 输入距离为 {p1p2, p1p3, p1p4, p2p3, p2p4, p3p4}
    //           {0,    1,    2,    3,    4,    5}
    // p3为原点
    points[2].v[0] = 0.0;
    points[2].v[1] = 0.0;
    points[2].v[2] = 0.0;

    // p1在X轴上
    points[0].v[0] = disPredef[1];  // p1p3 距离
    points[0].v[1] = 0.0;
    points[0].v[2] = 0.0;

    // 计算p4 夹角 p1p3p4为theta1
    double a = disPredef[1];  // p1p3
    double b = disPredef[5];  // p3p4
    double c = disPredef[2];  // p1p4
    double theta1 = acos((a*a+b*b-c*c)/(2*a*b));  // 余弦定理
    points[3].v[0] = b * cos(theta1);
    points[3].v[1] = b * sin(theta1);
    points[3].v[2] = 0;

    // p2  夹角 p1p2p4为theta2
    a = disPredef[1];  // p1p3
    b = disPredef[3];  // p2p3
    c = disPredef[0];  // p1p2
    double theta2 = acos((a*a+b*b-c*c)/(2*a*b));  // 余弦定理
    points[1].v[0] = b * cos(theta2);
    points[1].v[1] = b * sin(theta2);
    points[1].v[2] = 0.0;
}

//int MePtsHolderTypeY::matchPoints(std::vector<MePoint3D> &inPts, int outPtsIndex[], std::vector<MePoint3D> &outPts, const double eps)
//{
//    outPts.resize(4);
//    return calPt4PFromPefDefDis(disPredef, inPts, outPtsIndex, outPts.data(), eps);
//}

void MePtsHolderTypeY::calHolderTransformWith3Pt(std::vector<MePoint3D> &holderYPts, std::vector<MePoint3D> &baseHolderPts, Eigen::Matrix3d &R_YtoB, Eigen::Vector3d &T_YtoB)
{
    // 计算 nPose 个 矩阵，使用 [p1, p3, p4]
    MePoint3D baseHolderPts_3p[3] = {
        baseHolderPts[0], baseHolderPts[2], baseHolderPts[3]
    };
    MePoint3D holderPts_3p[3] = {
        holderYPts[0], holderYPts[2], holderYPts[3]
    };
    MeTransformCoord::rigidTransform3D(baseHolderPts_3p, holderPts_3p, 3, R_YtoB, T_YtoB);
}

bool MePtsHolderTypeY::calPtsFromFrontView(std::vector<MePoint3D> &inPts, std::vector<MePoint3D> &outPts)
{
    // 识别规则就是按照正视图放置的原则
    if (inPts.size() != 4) {
        return false;
    }
    outPts.resize(4);
    memcpy(outPts.data(), inPts.data(), 4*sizeof (MePoint3D));

    // 按照点从大到小排列，最大的为P1, 依次为P2, 剩下两个点 X小为P3, X大为P4
    sort(outPts.begin(), outPts.end(), cmpMePointYDsc);

    if (outPts[2].v[0] > outPts[3].v[0]) {
        MePoint3D tmpPt = outPts[2];
        outPts[2] = outPts[3];
        outPts[3] = tmpPt;
    }

    return true;
}


MePtsHolderTypeX::MePtsHolderTypeX()
{
    holderType = MePtsHolder::HolderTypeX;
}

void MePtsHolderTypeX::calPtsByDisPredef(MePoint3D points[])
{
    // 输入距离为 {p1p2, p1p3, p1p4, p2p3, p2p4, p3p4}
    //           {0,    1,    2,    3,    4,    5}
    // p1为原点
    points[0].v[0] = 0.0;
    points[0].v[1] = 0.0;
    points[0].v[2] = 0.0;

    // p2在X轴上
    points[1].v[0] = disPredef[0];  // p1p2 距离
    points[1].v[1] = 0.0;
    points[1].v[2] = 0.0;

    // 计算p3 夹角 p2p1p3为theta1
    double a = disPredef[0];  // p1p2
    double b = disPredef[1];  // p1p3
    double c = disPredef[3];  // p2p3
    double theta1 = acos((a*a+b*b-c*c)/(2*a*b));  // 余弦定理
    points[2].v[0] = b * cos(theta1);
    points[2].v[1] = b * sin(theta1);
    points[2].v[2] = 0;

    // p4  夹角 p2p1p4为theta2
    a = disPredef[0];  // p1p2
    b = disPredef[2];  // p1p4
    c = disPredef[4];  // p2p4
    double theta2 = acos((a*a+b*b-c*c)/(2*a*b));  // 余弦定理
    points[3].v[0] = b * cos(theta2);
    points[3].v[1] = b * sin(theta2);
    points[3].v[2] = 0.0;
}

bool MePtsHolderTypeX::calPtsFromFrontView(std::vector<MePoint3D> &inPts, std::vector<MePoint3D> &outPts)
{
    // 识别规则就是按照正视图放置的原则
    if (inPts.size() != 4) {
        return false;
    }
    outPts.resize(4);
    memcpy(outPts.data(), inPts.data(), 4*sizeof (MePoint3D));

    // 按照点从大到小排列，最大的为P1/P2, 剩下两个点 X小为P4, X大为P3
    sort(outPts.begin(), outPts.end(), cmpMePointYDsc);

    // X 小为P1
    if (outPts[0].v[0] > outPts[1].v[0]) {
        MePoint3D tmpPt = outPts[0];
        outPts[0] = outPts[1];
        outPts[1] = tmpPt;
    }

    // X小为P4
    if (outPts[2].v[0] < outPts[3].v[0]) {
        MePoint3D tmpPt = outPts[2];
        outPts[2] = outPts[3];
        outPts[3] = tmpPt;
    }
    return true;
}


MePtsHolderTFM102::MePtsHolderTFM102()
{
    // 默认值
    ptsPredef.resize(5);
    ptsPredef[0] = MePoint3D(25.6, 0, 246.8);
    ptsPredef[1] = MePoint3D(38.15, 0, 258.29);
    ptsPredef[2] = MePoint3D(45.22, 0, 265.37);
    ptsPredef[3] = MePoint3D(50.17, 12.12, 270.32);
    ptsPredef[4] = MePoint3D(63.59, -15, 283.74);

    static double distancePredefDefault[7] = { 27.0146, 36.1078, 55.0709, 13.9964, 29.9986,33.1012, 17.0154};
    memcpy(disPredef, distancePredefDefault, sizeof(double) * 7);
}

int MePtsHolderTFM102::matchPoints(std::vector<MePoint3D> &inPts, int outPtsIndex[5], std::vector<MePoint3D> &outPts, const double eps)
{
    outPts.resize(5);
    MePoint3D pt4p[4];
    // 先计算4点的映射
    int nMatched = MePtsHolder::calPt4PFromPefDefDis(disPredef, inPts, outPtsIndex, pt4p, eps);
    if (nMatched == 4) {
        // 计算预期的第五点坐标
        MePoint3D pt5 = pt4p[1] - pt4p[0];
        double d = pt5.norm();
        double d5 = disPredef[6];
        pt5 = pt4p[0] + pt5 * (d5/d);

        for (uint i = 0; i < inPts.size(); i++) {
            MePoint3D tmp = inPts[i] - pt5;
            if (tmp.norm() < eps) {
                outPts[0] = pt4p[0];
                outPts[1] = inPts[i];;
                outPts[2] = pt4p[1];
                outPts[3] = pt4p[2];
                outPts[4] = pt4p[3];

                outPtsIndex[4] = i;
                return 5;
            }
        }
    }

    return 0;
}
